package org.tastefuljava.hiketools.geo;

import java.util.Date;

public class TrackPoint {
    private static final double R = 6371030.0; // 11338.193804850347
    private static final double A = 6378137.0;
    private static final double A2 = A*A;
    private static final double B = 6356752.3142;
    private static final double B2 = B*B;

    private double lat;
    private double lng;
    private double h;
    private Date time;

    public static TrackPoint fromMn03(double x, double y, double ele,
            Date time) {
        y = y*1.E-6 - 0.2;
        x = x*1.E-6 - 0.6;
        double x2 = y*y;
        double x3 = x2*y;
        double y2 = x*x;
        double y3 = y2*x;
        double lon = 2.6779094
            + 4.728982*x
            + 0.791484*x*y
            + 0.1306*x*x2
            - 0.0436*y3;
        double lat = 16.9023892
            + 3.238272*y
            - 0.270978*y2
            - 0.002528*x2
            - 0.0447*y2*y
            - 0.0140*x3;
        /*
        double ele = alt + 49.55
            - 12.60*y
            - 22.64*x;
         */
        double h = ele;
        return new TrackPoint(lat/0.3600, lon/0.3600, h, time);
    }

    public TrackPoint(double lat, double lng, double h, Date time) {
        this.lat = lat;
        this.lng = lng;
        this.h = h;
        this.time = time;
    }

    public double getLat() {
        return lat;
    }

    public double getLng() {
        return lng;
    }

    public double getH() {
        return h;
    }

    public Date getTime() {
        return time;
    }

    public Mn03Point toTrackpoint() {
        return Mn03Point.fromWGS84(lat, lng, h, time);
    }

    public double distance(TrackPoint p2) {
        double lat1 = Math.toRadians(getLat());
        double lat2 = Math.toRadians(p2.getLat());
        double lon1 = Math.toRadians(getLng());
        double lon2 = Math.toRadians(p2.getLng());
        double cosP = Math.cos(lat1);
        double sinP = Math.sin(lat1);
        double s2 = A2 * cosP * cosP + B2 * sinP * sinP;
        double s = Math.sqrt(s2);
        double sq1 = (lat1 - lat2) * (getH() + A2*B2/(s*s2));
        double sq2 = (lon1 - lon2) * (getH() + A2/s)*cosP;
        return Math.sqrt(sq1 * sq1 + sq2 * sq2);
    }

    public double distanceToGreatCircle(TrackPoint a, TrackPoint b) {
        double lata = Math.toRadians(a.getLat());
        double lnga = Math.toRadians(a.getLng());
        double latb = Math.toRadians(b.getLat());
        double lngb = Math.toRadians(b.getLng());
        double latp = Math.toRadians(getLat());
        double lngp = Math.toRadians(getLng());
        double sinlata = Math.sin(lata);
        double coslata = Math.cos(lata);
        double sinlnga = Math.sin(lnga);
        double coslnga = Math.cos(lnga);
        double sinlatb = Math.sin(latb);
        double coslatb = Math.cos(latb);
        double sinlngb = Math.sin(lngb);
        double coslngb = Math.cos(lngb);
        double sinlatp = Math.sin(latp);
        double coslatp = Math.cos(latp);
        double sinlngp = Math.sin(lngp);
        double coslngp = Math.cos(lngp);
        double costh = sinlata*sinlatb
                + coslata*coslatb*(coslnga*coslngb+sinlnga*sinlngb);
        double sin2th = 1-costh*costh;
        if (sin2th < 1.0E-20) {
            // return distance from a to p
            double costhp = sinlata*sinlatp
                    + coslata*coslatp*(coslnga*coslngp+sinlnga*sinlngp);
            return Math.acos(costhp)*(R+getH());
        }
        double num = sinlata*(coslatb*coslatp*coslngb*sinlngp
                - coslatb*coslatp*sinlngb*coslngp)
            + coslata*coslnga*(coslatb*sinlatp*sinlngb
                - sinlatb*coslatp*sinlngp)
            + coslata*sinlnga*(sinlatb*coslatp*coslngp
                - coslatb*sinlatp*coslngb);
        double sinr = Math.abs(num)/Math.sqrt(sin2th);
        return (R+getH())*Math.asin(sinr);
    }

    public double sphericalDistance(TrackPoint other) {
        double avh = (h + other.getH())/2;
        double lat1 = Math.toRadians(lat);
        double lat2 = Math.toRadians(other.getLat());
        double lon1 = Math.toRadians(lng);
        double lon2 = Math.toRadians(other.getLng());
        double a = Math.sin(lat1) * Math.sin(lat2) +
                Math.cos(lat1) * Math.cos(lat2) * Math.cos(lon1-lon2);
        double c = Math.acos(a);
        return (R + avh) * c;
    }

    public double vincentyDistance(TrackPoint p2) {
        double lat1 = Math.toRadians(lat);
        double lat2 = Math.toRadians(p2.getLat());
        double lon1 = Math.toRadians(lng);
        double lon2 = Math.toRadians(p2.getLng());
        double a = 6378137.0, b = 6356752.3142, f = 1 / 298.257223563;  // WGS-84 ellipsoid params
        double L = lon2 - lon1;
        double U1 = Math.atan((1 - f) * Math.tan(lat1));
        double U2 = Math.atan((1 - f) * Math.tan(lat2));
        double sinU1 = Math.sin(U1), cosU1 = Math.cos(U1);
        double sinU2 = Math.sin(U2), cosU2 = Math.cos(U2);

        double lambda = L, lambdaP, iterLimit = 100, cosSqAlpha, sinSigma,
                cosSigma, cos2SigmaM, sigma, sinLambda, cosLambda;
        do {
            sinLambda = Math.sin(lambda);
            cosLambda = Math.cos(lambda);
            sinSigma = Math.sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda)
                    + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
            if (sinSigma == 0) {
                return 0;  // co-incident points
            }
            cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
            sigma = Math.atan2(sinSigma, cosSigma);
            double sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
            cosSqAlpha = 1 - sinAlpha * sinAlpha;
            cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
            if (Double.isNaN(cos2SigmaM)) {
                cos2SigmaM = 0;  // equatorial line: cosSqAlpha=0 (§6)
            }
            double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
            lambdaP = lambda;
            lambda = L + (1 - C) * f * sinAlpha
                    * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
        } while (Math.abs(lambda - lambdaP) > 1e-12 && --iterLimit > 0);
        if (iterLimit == 0) {
            return Double.NaN;
        }
        double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
        double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
        double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
        double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)
                - B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
        double s = b * A * (sigma - deltaSigma);
        return s;
    }
}
